/*
 * main_controller.cc
 *
 *  Created on: Apr 9, 2010
 *      Author: Aron
 */

#include "xtask.h"
#include "xqueue.h"
#include "log.h"
#include "main_controller.h"
#include "ir_manager.h"
#include "ball_catcher.h"
#include "robo_control.h"
#include "modules/Vision/VisionManager.h"
#include "modules/Util/ConfigFile.h"
#include "modules/Mapping/gridmap.h"
#include <cmath>

using namespace kthrobot;
using namespace std;

namespace Group5 {

Main_Controller::Main_Controller()
: XTask("MainController"), ir_manager_(&sensors_reader_),
  motor1_("left",0), motor2_("right",3) {
  SetPeriodHz(hz_);
  can_move_forward_ = true;
  can_move_left_  = false;
  can_move_right_ = false;
  mode_ = 0;
}

Main_Controller::~Main_Controller() {
  Join();
}





void Main_Controller::TaskCode() {
  //working version
  bool have_turned = false;
  double dist = 0;
  double target_ang = 0;
  double rspeed = 0;
  double lspeed = 0;
  current_rspeed = 0;
  current_lspeed = 0;
  double rcorrect = 0;
  double lcorrect = 0;
  int detect_left = 0;
  int detect_right = 0;
  ang = 0;

  double last_x = 0;
  double last_y = 0;
  double x_diff = 0;
  double y_diff = 0;

  double L = 18.2;

  double x = 0;
  double y = 0;
  int state = LOGIC;
  int previous_state = -1;
  can_move_forward_ = true;
  sensors_reader_.Start();
  motor1_.Start();
  motor2_.Start();

  //wait for sensors
  TimeTools::WaitMS(1000);

  if (USE_CAMERA) VisionManager::getInstance().Up();
  DetectionResults ball_results;

  Gridmap gm;
  Position* pos;

  while(!MustStop()) {


    //Log::Write( "Front range: %d", ir_manager_.FrontRange() );

    // Display current state if it has changed
    if (LOG_STATES && state != previous_state) {
      previous_state = state;
      Log::Write("Current state: %i", state);
    }

    // Update speed and position
    current_lspeed = motor1_.GetLastSpeed();
    current_rspeed = -motor2_.GetLastSpeed();
    double s1 = current_lspeed;
    double s2 = current_rspeed;

    if (s1 == s2) s2 -= 0.1;

    double curv_rad = ((s1+s2)/(s1-s2))*(L/2);
    //Log::Write("real rad: %f", curv_rad);
    if ( curv_rad > 100 || curv_rad < -100 ) curv_rad = 3000000;
    if ( curv_rad == 0 ) curv_rad = 0.0001;
    double robot_speed = (s1+s2)/2;

    double distance = robot_speed;
    double theta = distance / curv_rad;

    ang += theta;
    ang = fmod(ang, 2*3.14);
    if ( ang < 0 ) ang = 2*pi + ang;

    double xdiff = curv_rad - curv_rad*cos(theta);
    double ydiff = curv_rad*sin(theta);


    //Log::Write( "cost: %f, sint: %f", curv_rad - curv_rad*cos(theta), curv_rad*sin(theta) );

    double xadd = xdiff*cos(ang) - ydiff*sin(ang);
    double yadd = xdiff*sin(ang) + ydiff*cos(ang);

    x += xadd;
    y += yadd;

    x_diff = abs(last_x - x);
    y_diff = abs(last_y - y);

    if ( x_diff < 1 ) {
      int rest = (int)x % 40;
      //Log::Write("Rest x: %d", rest);
      if ( rest < 20 ) x -= rest;
      else x += 40 - rest;
    }
    if ( y_diff < 1 ) {
      int rest = (int)y % 40;
      //Log::Write("Rest y: %d", rest);
      if ( rest < 20 ) y -= rest;
      else y += 40 - rest;
    }

    // Update the grid
    pos = gm.current_position();
    if( y > (*pos).y+30 ){
      gm.move(NORTH);
    }
    else if ( y < (*pos).y-30 ) {
      gm.move(SOUTH);
    }
    else if( x > (*pos).x+30 ){
      gm.move(EAST);
    }
    else if ( x < (*pos).x-30 ) {
      gm.move(WEST);
    }

    // Processing
    switch(state){
    /***************************************************************
                 Stationary, needs to decide where to go next
     ***************************************************************/
    case LOGIC:
      rspeed = 0;
      lspeed = 0;
      dist = 0;

      rcorrect = 0;
      lcorrect = 0;
      if(MotorsStopped()){
        // We are stopped, let's think...
        if (LOG_OPENINGS) Log::Write( " %s, %s, %s", (can_move_left_)?"left":" ",(can_move_forward_)?"forward":" ",(can_move_right_)?"right":"");

        // Define the strategy of the robot
        Order* order = gm.get_order();
        // Try to follow the given orders by order of priority
        bool order_succeed;
        do {
            order_succeed = true;
            if (order->direction == FORWARD && can_move_forward_) {
                if (LOG_DECISIONS) Log::Write("Decision: Move forward");
                state = FORWARD40;
            } else if (order->direction == LEFT && can_move_left_) {
                if (LOG_DECISIONS) Log::Write("Decision: Move left");
                state = TURNLEFT;
            } else if (order->direction == RIGHT && can_move_right_) {
                if (LOG_DECISIONS) Log::Write("Decision: Move right");
                state = TURNRIGHT;
            } else if (order->direction == BACKWARD) {
                if (LOG_DECISIONS) Log::Write("Decision: Move backward");
                state = UTURN;
            } else order_succeed = false;
        } while(!order_succeed && (order = order->next));
        // Fallback
        if (!order_succeed) state = UTURN;
      }
      break;

      /***************************************************************
                 Stop current motion and then continue forward
                 Last step of a turn movement
       ***************************************************************/
    /*
    case ALIGN:
      int fl = ir_manager_.FrontLeftRange();
      int rl = ir_manager_.RearLeftRange();
      int fr = ir_manager_.FrontRightRange();
      int rr = ir_manager_.RearRightRange();

      if ( fl >= 5 && fl < 25 && rl >= 5 && rl < 25 ) {
          if( fl > rl+1 ) {
            rspeed = 1.0;
            lspeed = -1.0;
          }
          else {
            rspeed = 0;
            lspeed = 0;
            state = LOGIC;
          }
      }
    */

    case STOP:
      can_move_left_ = false;
      can_move_right_ = false;
      can_move_forward_ = true;
      rspeed = 0;
      lspeed = 0;
      dist = 0;
      rcorrect = 0;
      lcorrect = 0;
      detect_right = 0;
      detect_left = 0;

      state = FORWARD40;


      break;

      /**************************************************************
                 Move forward 40 cm and align against any wall it can detect
       ***************************************************************/
    case FORWARD40:
      //reset angle  when possible, 90deg to both walls
      ResetAngBothWalls();

      can_move_forward_ = true;

      lcorrect = 0;
      rcorrect = 0;
      //update dist
      if(abs(yadd) > abs(xadd)){
        dist = dist+1.2*abs(yadd);
      }
      else {
        dist = dist+1.2*abs(xadd);
      }

      /*
       * Check for a wall in the front
       */
      if(ir_manager_.FrontFree()){
        // If no wall detected, go forward full speed
        rspeed = 4;
        lspeed = 4;
      }
      else if(!ir_manager_.FrontClose()){
        // If a wall is detected but not close enough slow down
        rspeed = 2;
        lspeed = 2;
      }
      else if(ir_manager_.FrontTooClose()){
        // If we are too close from the wall back off
        rspeed = -2;
        lspeed = -2;
      }

      /*
       * Look for openings on the sides during movement
       * Check data from both sensors multiple times to have robust detection
       */
      // Regular check if we are not out of a turn
      if(have_turned == false){
        if(ir_manager_.FrontRightFree() && !can_move_right_){
          detect_right++;
          rspeed = 2;
          lspeed = 2;
          if (LOG_MULTIPLE_SIDE_DETECTIONS) Log::Write( "right: %i",detect_right);
          // Reset position estimate according to the opening
        }
        else
          detect_right = 0;

        if(can_move_right_ || can_move_left_){
          rspeed = 2;
          lspeed = 2;
        }

        if(ir_manager_.FrontLeftFree()  && !can_move_left_){
          detect_left++;
          rspeed = 2;
          lspeed = 2;
          if (LOG_MULTIPLE_SIDE_DETECTIONS) Log::Write( "left: %i",detect_left);
          // Reset position estimate according to the opening
        }
        else
          detect_left = 0;

        // ... and then by the back side sensors
        if ( ir_manager_.RearRightFree() && detect_right >= 4 && !can_move_right_){
          can_move_right_ = true;
          detect_right = 0;
          dist = 36;
        }
        if ( ir_manager_.RearLeftFree() && detect_left >= 4 && !can_move_left_){
          can_move_left_ = true;
          detect_left = 0;
          dist = 36;
        }
      }
      // If we are out of a turn, special check
      else {
        if (((dist > 30 && ir_manager_.LongRearLeftFree()) || (dist > 20 && ir_manager_.LongFrontLeftFree())) && can_move_left_ == false){
          can_move_left_ = true;
        }
        if (((dist > 30  && ir_manager_.LongRearRightFree()) || (dist > 20 && ir_manager_.LongFrontRightFree())) && can_move_right_ == false){
          can_move_right_ = true;
        }
      }

      // Resetting the detect* var if the sensors does not see openings
      if (!ir_manager_.FrontLeftFree()) {
        detect_left = 0;
        can_move_left_ = false;
      }
      if (!ir_manager_.FrontRightFree()) {
        detect_right = 0;
        can_move_right_ = false;
      }


      if((!have_turned || dist > 5) && ir_manager_.FrontClose()){
        if (ir_manager_.LongRearLeftFree() || ir_manager_.LongFrontLeftFree()){
          can_move_left_ = true;
        }
        if (ir_manager_.LongRearRightFree() || ir_manager_.LongFrontRightFree()){
          can_move_right_ = true;
        }
        rspeed = 0;
        lspeed = 0;
        dist = 41;
        lcorrect = 0;
        rcorrect = 0;
        detect_right = 0;
        detect_left = 0;
        can_move_forward_ = false;
      }

      /*
       * Stop after 40cm
       */
      //Log::Write( "dist: %f",dist);

      if(dist > 40) {
        dist = 0;
        have_turned = false;
        if(!ir_manager_.StatFrontFree()){
          can_move_forward_ = false;
          if (ir_manager_.LongRearLeftFree() || ir_manager_.LongFrontLeftFree()){
            can_move_left_ = true;
          }
          if (ir_manager_.LongRearRightFree() || ir_manager_.LongFrontRightFree()){
            can_move_right_ = true;
          }
        }
        else{
          can_move_forward_ = true;
        }
        if(can_move_forward_ && !can_move_left_ && !can_move_right_){
          state = FORWARD40;
        }
        else {
          rspeed = 0;
          lspeed = 0;
          lcorrect = 0;
          rcorrect = 0;
          state = LOGIC;
        }

        break;
      }

      // If walls are detected on both sides use both data for
      // correcting the trajectory
      if (USE_CAMERA && mode_ == BALLCATCHING){
        ball_results = VisionManager::getInstance().getBallResult();
      }
      if (USE_CAMERA && mode_ == BALLCATCHING && ball_results.detected) {
        Log::Write("BallCatcher: Ball detected");
        Log::Write("BallCatcher: x = %i, y = %i, w = %i, h = %i, s = %i",
            ball_results.x, ball_results.y, ball_results.w, ball_results.h, ball_results.size);

        // Otherwise go forward aiming for the ball

        Log::Write("BallCatcher: Catching the ball!");
        // Coordinates of the center of the detected ball
        uint8_t center_x = ball_results.x; //+ball_results.w/2;
        Log::Write("BallCatcher: computed center = %d", center_x);
        //uint8_t center_y = ball_results.y+ball_results.w/2;
        // Horizontal distance to the center, between -1 (left) and +1 (right)
        int IMG_X = Constants::get().IMG_X;
        double diff = (2*center_x - IMG_X) / (double) IMG_X;
        Log::Write("BallCatcher: diff = %f", diff);

        rspeed = (1-0.1*diff)*BASE_SPEED;
        lspeed = (1+0.1*diff)*BASE_SPEED;
        rcorrect = 0;
        lcorrect = 0;

      }
      else if(!ir_manager_.ShortFrontRightFree() && !ir_manager_.ShortRearRightFree() &&
          !ir_manager_.ShortFrontLeftFree() && !ir_manager_.ShortRearLeftFree()) {
        double corr = GetCorrectionBothWalls();
        corr += GetRangeCorrBothWalls();
        rcorrect = corr;
        lcorrect = -corr;
      }

      // If only the left wall is detected use it
      else if(!ir_manager_.ShortFrontLeftFree() && !ir_manager_.ShortRearLeftFree()) {
        double corr = GetCorrectionOneWall(ir_manager_.FrontLeftRange(),ir_manager_.RearLeftRange());
        corr += GetRangeCorrOneWall(ir_manager_.FrontLeftRange(),ir_manager_.RearLeftRange());
        rcorrect = corr;
        lcorrect = -corr;
      }

      // If only the right wall is detected use it
      else if(!ir_manager_.ShortFrontRightFree() && !ir_manager_.ShortRearRightFree()) {
        double corr = GetCorrectionOneWall(ir_manager_.FrontRightRange(),ir_manager_.RearRightRange());
        corr += GetRangeCorrOneWall(ir_manager_.FrontRightRange(),ir_manager_.RearRightRange());
        rcorrect = -corr;
        lcorrect = corr;
      }

      break;

      /***************************************************************
                 Turn left 90 degrees
       ***************************************************************/
    case TURNLEFT:
      if (!have_turned) {
        // Try to estimate the current angle to the wall
        target_ang = GetClosestStraightAng()-0.5*pi-0.2;//ir_manager_.EstimateAngleToRightWall();
        if(target_ang < 0)
          target_ang += 2*pi;
        else if (target_ang > 2*pi)
          target_ang -= 2*pi;
        // Command the motors to turn left

        rspeed = 1.4;
        lspeed = -1.4;
        have_turned = true;
      }

      // Turn until reaching 90°
      if(have_turned && ang > (target_ang) && ang < (target_ang+0.4)) {
        state = STOP;
        rspeed = 0;
        lspeed = 0;
        break;
      }
      break;

      /***************************************************************
                 Turn right 90°
       ***************************************************************/
    case TURNRIGHT:
      if (!have_turned) {
        // Try to estimate the current angle to the wall
        target_ang = GetClosestStraightAng()+0.5*pi-0.2;//ir_manager_.EstimateAngleToRightWall();
        if(target_ang < 0)
          target_ang += 2*pi;
        else if (target_ang >= 2*pi)
          target_ang -= 2*pi;
        // Command the motors to turn right
        rspeed = -1.4;
        lspeed = 1.4;
        have_turned = true;
      }

      // Turn until reaching 90°
      if(have_turned && ang > (target_ang) && ang < (target_ang+0.4)) {
        state = STOP;
        rspeed = 0;
        lspeed = 0;
        break;

      }
      break;

      /***************************************************************
                 U-turn
       ***************************************************************/
    case UTURN:
      Log::Write("ang: %f, target %f", ang, target_ang);
      if (!have_turned) {
        // Try to estimate the current angle to the wall
        target_ang = GetClosestStraightAng()-pi-0.2;
        // Command the motors to turn the best way
        if((ir_manager_.FrontRightRange()+ir_manager_.RearRightRange()) > (ir_manager_.FrontLeftRange()+ir_manager_.RearLeftRange())){
          rspeed = -1.4;
          lspeed = 1.4;

        }
        else{
          rspeed = 1.4;
          lspeed = -1.4;

        }
        if(target_ang < 0)
          target_ang += 2*pi;
        else if (target_ang >= 2*pi)
          target_ang -= 2*pi;
        // Command the motors to turn right
        have_turned = true;
      }

      // Turn until reaching 180°
      if(have_turned && ang > (target_ang) && ang < (target_ang+0.4)) {
        state = STOP;
        rspeed = 0;
        lspeed = 0;
        break;

      }
      break;
    }
    motor1_.SetTargetSpeed(lspeed+0.5*lcorrect);
    motor2_.SetTargetSpeed(-1*(rspeed+0.5*rcorrect));
    double targetRelSpeed = CalcRelTargetSpeed();
    motor1_.Set_uP_mc(targetRelSpeed);
    motor2_.Set_uP_mc(targetRelSpeed);

    WaitPeriod();
  }

  if (USE_CAMERA) VisionManager::getInstance().Down();

};

//for the p controller
double Main_Controller::CalcRelTargetSpeed(){
  return 0; //(current_lspeed+current_rspeed)/2;
}


//correction for angle vs one wall
double Main_Controller::GetCorrectionOneWall(int front, int rear){
  int absErr = std::fabs(front-rear);
  //max value for the correction
  if(absErr > 15){
    return (absErr/(front-rear));
  } // gives a value of 0.1-1
  else if(absErr > 1){
    return (double(front-rear))/15.0;
  }
  return 0;
}


//correction for range vs one wall
double Main_Controller::GetRangeCorrOneWall(int front, int rear){
  if (front <= 4){
    return -2;
  }
  double dist = double(front)-double(rear);
  //calculate distance from wall
  if(dist != 0){
    double angle = std::tan(dist/17);
    dist =  std::cos(angle)*(double(front)+double(rear))/2;
  }
  else {
    dist = (double(front)+double(rear))/2;
  }
  //if too far... use a fixed value for now

  if(dist > 14){
    return 0.2;
  }
  else if(dist > 11){
    return 0;
  } //value of 0-1 if too close
  return -(11-dist)/12;
}


//correction for angle vs both wall, same as one... just use data from both sides
//and take the m value
double Main_Controller::GetCorrectionBothWalls(){
  int frontL = ir_manager_.FrontLeftRange();
  int rearL = ir_manager_.RearLeftRange();
  int frontR = ir_manager_.FrontRightRange();
  int rearR = ir_manager_.RearRightRange();
  int err = (frontL-rearL+rearR-frontR)/2;
  int absErr = std::fabs(err);
  if (frontL <= 4){
      return 2;
  }
  else if (frontR <= 4){
        return -2;
  }
  if(absErr > 12){
    return (absErr/err);
  }
  else if(absErr > 1){
    return double(err)/12.0;
  }
  return 0;
}
double Main_Controller::GetClosestStraightAng(){
  if(ang < pi/4 || ang >= pi*7/4){return 0;}
  else if(ang >= pi/4 && ang < pi*3/4){return pi/2;}
  else if(ang >= pi*3/4 && ang < pi*5/4){return pi;}
  else {return pi*3/2;}
}
void Main_Controller::ResetAngBothWalls(){
  if((ir_manager_.FrontLeftRange()-ir_manager_.RearLeftRange()) == 0 ||
      (ir_manager_.FrontRightRange()-ir_manager_.RearRightRange()) == 0){
      ang = GetClosestStraightAng();
    if (LOG_RESET_ANGLE) Log::Write("reset ang");
  }
}

//correction for range vs both walls
double Main_Controller::GetRangeCorrBothWalls(){
  int frontL = ir_manager_.FrontLeftRange();
  int rearL = ir_manager_.RearLeftRange();
  int frontR = ir_manager_.FrontRightRange();
  int rearR = ir_manager_.RearRightRange();

  double meanL = (double(frontL)+double(rearL))/2;
  double meanR = (double(frontR)+double(rearR))/2;
  //calculate dist to  each wall
  double distL = double(frontL)-double(rearL);
  double angle = 0;
  if(distL != 0){
    angle = std::tan(distL/17);
    distL =  std::cos(angle)*meanL;
  }
  else {
    distL = meanL;
  }
  double distR = double(frontR)-double(rearR);
  if(distR != 0){
    angle = std::tan(distR/17);
    distR =  std::cos(angle)*meanR;
  }
  else {
    distR = meanR;
  }
  // correct if closer than 10 cm
  if(distL < 11){
    return -(11-distL)/12;
  }
  else if(distR < 11){
    return (11-distR)/12;
  }
  return 0;
}


bool Main_Controller::MotorsStopped() {
  // Check if the robot is actually standing still
  return current_lspeed == 0 && current_rspeed == 0;
}


double Main_Controller::GetDeltaAngle() {
  // Estimate the rotation angle since last period
  return 0.0;
}

void Main_Controller::SetMode(int m){
  mode_ = m;
}

};
